#include <list>
#include "HenningCamera.h"

Camera::Camera(TrackingThreshold primaryColor, TrackingThreshold secondaryColor, CameraLib::MyOrder me, int horizontalServo, int verticalServo, InputHandler* input){
	m_cam = new CameraLib::Camera(primaryColor, secondaryColor, me);
	//Servo::SetDefaultDigitalModule(5);
	m_servoHorizontal = new Servo(6, 3);
	m_servoVertical = new Servo(6, 2); 
	m_pidHorizontal = new PID(.01, .001, .001, 1, 0);
	m_pidVertical = new PID(.01, .001, .001, 1, 0);
	m_panH = PAN_VALUE;
	m_panV = .5;
	m_input = input;
	m_timeStamp = -1;
}

void Camera::Calculate() {
	m_cam->Calculate();
	/*for (ITERATOR(CameraLib::Flag) i = m_cam->flags.begin(); i != m_cam->flags.end(); i++) {
		//printf("d=%f\n", (*i).GetDistance());
		//(*i).print();
	}*/
	
	bool foesPresent = false;
	for (ITERATOR(CameraLib::Flag) f = m_cam->flags.begin(); f != m_cam->flags.end(); ++f)
		if ((*f).IsEnemy()) foesPresent = true;
	
	if (m_timeStamp != m_cam->timeStamp) {
		if (foesPresent) {
			target = AcquireTarget(m_cam->flags);
			hasTarget = true;
		} else {
			hasTarget = false;
		}
	}
}

void Camera::Thread() {
	char str[] = "Locate Targets";
	//Task* t = new Task(str, HenningThreadCamera);
	//t->Start();
}

void Camera::Run() {
	while (true) Calculate();
}

bool Camera::CanFire() {
	if (hasTarget)
		//TODO: Get real distance (!5)
		if (target.GetDistance() < 5)
			return true;
	return false;
}

void Camera::Pan() {
	if ((m_panH > 0) && (GET_SERVO_VALUE(m_servoHorizontal) >= 1 - PAN_VALUE)) {
		m_panH = -PAN_VALUE;
		m_panV = .5;
	}
	else if ((m_panH < 0) && (GET_SERVO_VALUE(m_servoHorizontal) <= -1 + PAN_VALUE)) {
		m_panH = PAN_VALUE;
		m_panV = 0;
	}
	SetServos(GET_SERVO_VALUE(m_servoHorizontal) + m_panH, m_panV);
}

void Camera::FollowTarget() {
	printf("track");
	if (m_timeStamp != m_cam->timeStamp) {
		printf("ing\n");
		m_timeStamp = *&m_cam->timeStamp;
		if (hasTarget) {
			float x = target.GetX(), y = target.GetY();
			float moveH = 0, moveV = 0;
			
			if (-.2 >= x || x >= .2) {
				if (x > 0) {
					if (x < .5)			{moveH =  -.0125;}
					else if (x < .8)	{moveH =  -.025;}
					else				{moveH =  -.05;}
				} else if (x < 0) {
					if (x > -.5) 		{moveH = .0125;}
					else if (x > -.8) 	{moveH = .025;}
					else				{moveH = .05;}
				}
			}
			
			if (-.2 >= y || y >= .2) {
				if (y > 0) {
					if (y < .5)			{moveV =  -.0125;}
					else if (y < .8)	{moveV =  -.025;}
					else				{moveV =  -.05;}
				} else if (y < 0) {
					if (y > -.5) 		{moveV = .0125;}
					else if (y > -.8) 	{moveV = .025;}
					else				{moveV = .05;}
				}
			}
			
			printf("%f %f\n", x, moveH);
			
			SetServos(GET_SERVO_VALUE(m_servoHorizontal) + moveH, GET_SERVO_VALUE(m_servoVertical) + moveV);
		}
	}
}

CameraLib::Flag Camera::AcquireTarget(std::list<CameraLib::Flag> flags) {
	//TODO: Replace with real code (! choosing the biggest object)
	CameraLib::Flag flag = (*flags.begin());
	float biggest = 0;
	for (ITERATOR(CameraLib::Flag) f = flags.begin(); f != flags.end(); ++f) {
		if (((*f).GetArea() > biggest) && (*f).IsEnemy()){
			biggest = (*f).GetArea();
			flag = (*f);
		}
	}
	return flag;
}

float Camera::GetAngle() {
	return GET_SERVO_VALUE(m_servoHorizontal);
}

void Camera::SetServos(float horizontal, float vertical) {
	m_servoHorizontal->Set(horizontal / 2 + .5);
	m_servoVertical->Set(vertical / 2 + .5);
}

void Camera::print() {
	//printf("Camera: x=%e y=%e distance=%e angle=%e\n", m_x, m_y, GetDistance(), GetAngle());
}
